#include "aabb.h"
#include "ray.h"
#include "matrix.h"

#include <algorithm>
#include <limits>

using namespace krryn::math;

aabb::aabb(){empty();}

aabb::aabb(vector3f a_Min, vector3f a_Max) : m_Min(a_Min), m_Max(a_Max){}

aabb::aabb(const aabb &a_Box) : m_Min(a_Box.m_Min), m_Max(a_Box.m_Max){}

void aabb::empty(){
	m_Min.m_X = m_Min.m_Y = m_Min.m_Z =  std::numeric_limits<float>::infinity();
	m_Max.m_X = m_Max.m_Y = m_Max.m_Z = -std::numeric_limits<float>::infinity();
}

void aabb::expand(const vector3f &a_Point){
	m_Max.m_X = std::max<float>(a_Point.m_X, m_Max.m_X);
	m_Max.m_Y = std::max<float>(a_Point.m_Y, m_Max.m_Y);
	m_Max.m_Z = std::max<float>(a_Point.m_Z, m_Max.m_Z);

	m_Min.m_X = std::min<float>(a_Point.m_X, m_Min.m_X);
	m_Min.m_Y = std::min<float>(a_Point.m_Y, m_Min.m_Y);
	m_Min.m_Z = std::min<float>(a_Point.m_Z, m_Min.m_Z);
}

void aabb::transform(matrix &a_Matrix){
	m_Max = a_Matrix.transform(m_Max);
	m_Min = a_Matrix.transform(m_Min);
}

aabb aabb::combine(const aabb &a_Other) const{
	aabb l_Box;

	l_Box.m_Max.m_X = std::max<float>(a_Other.m_Max.m_X, m_Max.m_X);
	l_Box.m_Max.m_Y = std::max<float>(a_Other.m_Max.m_Y, m_Max.m_Y);
	l_Box.m_Max.m_Z = std::max<float>(a_Other.m_Max.m_Z, m_Max.m_Z);

	l_Box.m_Min.m_X = std::min<float>(a_Other.m_Min.m_X, m_Min.m_X);
	l_Box.m_Min.m_Y = std::min<float>(a_Other.m_Min.m_Y, m_Min.m_Y);
	l_Box.m_Min.m_Z = std::min<float>(a_Other.m_Min.m_Z, m_Min.m_Z);

	return l_Box;
}

aabb aabb::intersect(const aabb &a_Other) const{
	aabb l_Box;

	l_Box.m_Max.m_X = std::min<float>(a_Other.m_Max.m_X, m_Max.m_X);
	l_Box.m_Max.m_Y = std::min<float>(a_Other.m_Max.m_Y, m_Max.m_Y);
	l_Box.m_Max.m_Z = std::min<float>(a_Other.m_Max.m_Z, m_Max.m_Z);

	l_Box.m_Min.m_X = std::max<float>(a_Other.m_Min.m_X, m_Min.m_X);
	l_Box.m_Min.m_Y = std::max<float>(a_Other.m_Min.m_Y, m_Min.m_Y);
	l_Box.m_Min.m_Z = std::max<float>(a_Other.m_Min.m_Z, m_Min.m_Z);

	return l_Box;
}

vector3f aabb::position() const{
	return (m_Max + m_Min) / 2.f;
}

vector3f aabb::size() const{
	return m_Max - m_Min;
}

vector3f aabb::center() const{
	return (m_Max + m_Min) * .5f;
}

float aabb::area() const{
	vector3f l_Size = size();

	return (l_Size.m_X * l_Size.m_Y) 
		+ (l_Size.m_Y * l_Size.m_Z)
		+ (l_Size.m_Z * l_Size.m_X);
}

bool aabb::contains(const vector3f &a_Point) const{
	return (a_Point.m_X >= m_Min.m_X) && (a_Point.m_X <= m_Max.m_X) &&
		(a_Point.m_Y >= m_Min.m_Y) && (a_Point.m_Y <= m_Max.m_Y) &&
		(a_Point.m_Z >= m_Min.m_Z) && (a_Point.m_Z <= m_Max.m_Z);
}

bool aabb::contains_almost(const vector3f &a_Point) const{
	return (a_Point.m_X >= (m_Min.m_X - 0.0001)) && (a_Point.m_X <= (m_Max.m_X + 0.0001)) &&
		(a_Point.m_Y >= (m_Min.m_Y - 0.0001)) && (a_Point.m_Y <= (m_Max.m_Y + 0.0001)) &&
		(a_Point.m_Z >= (m_Min.m_Z - 0.0001)) && (a_Point.m_Z <= (m_Max.m_Z + 0.0001));
}

aabb aabb::get_aabb() const{
	aabb l_Box;
	l_Box.expand(m_Min);
	l_Box.expand(m_Max);
	return l_Box;
}

bool aabb::intersects_with_ray(ray &a_Ray) const{
	float    l_Distance[6];
	vector3f l_IntersectionPoints[6];

	bool l_Result = false;

	for(size_t i = 0; i < 6; i++){
		l_Distance[i] = -1.f;
	}

	vector3f l_V1 = m_Min, l_V2 = m_Max;

	if(a_Ray.m_Direction.m_X != 0.f){
		float l_Reci = 1.f / a_Ray.m_Direction.m_X;
		l_Distance[0] = (l_V1.m_X - a_Ray.m_Origin.m_X) * l_Reci;
		l_Distance[3] = (l_V2.m_X - a_Ray.m_Origin.m_X) * l_Reci;
	}

	if(a_Ray.m_Direction.m_Y != 0.f){
		float l_Reci = 1.f / a_Ray.m_Direction.m_Y;
		l_Distance[1] = (l_V1.m_Y - a_Ray.m_Origin.m_Y) * l_Reci;
		l_Distance[4] = (l_V2.m_Y - a_Ray.m_Origin.m_Y) * l_Reci;        
	}

	if(a_Ray.m_Direction.m_Z != 0.f){
		float l_Reci = 1.f / a_Ray.m_Direction.m_Z;
		l_Distance[2] = (l_V1.m_Z - a_Ray.m_Origin.m_Z) * l_Reci;
		l_Distance[5] = (l_V2.m_Z - a_Ray.m_Origin.m_Z) * l_Reci;        
	}

	for(size_t i = 0; i < 6; i++){
		if(l_Distance[i] > 0.f){
			l_IntersectionPoints[i] = a_Ray.m_Origin + l_Distance[i] * a_Ray.m_Direction;

			if(contains_almost(l_IntersectionPoints[i])){
				if(l_Distance[i] < a_Ray.m_Distance){
					a_Ray.m_Distance = l_Distance[i];
					return true;
				}
			}
		}
	}

	return l_Result;
}
